using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;
using System.Text;

namespace Diversity.Robotics.Navigation
{
    public class RobotFromLogFile : Robot
    {
        private List<double[]> _stateDoubles = new List<double[]>();
        private List<double[]> _readingDoubles = new List<double[]>();
        private List<double[]> _velocityDoubles = new List<double[]>();
        private int _currentStep = 0;

        public double minX = double.MaxValue;
        public double minY = double.MaxValue;
        public double maxX = double.MinValue;
        public double maxY = double.MinValue;
        public double maxRange = double.MinValue;
        public int numBeams = 0;
        public string status = "Awaiting File...";
        public int numOdometry = 0;
        public int numScans = 0;
        public bool success = false;

        private BeamModel _beamModel;
        private ConeModel1Gaussian _coneModel;
        private double _fov;
        private double _offsetX;
        private double _offsetY;
        private double _scale;

        public RobotFromLogFile(Pose2D state, double width, double length, Map globalMap, string logFileName, BeamModel beamModel, ConeModel1Gaussian coneModel, double fov, double offsetX, double offsetY, double scale)
            : base(state, width, length, globalMap)
        {
            _beamModel = beamModel;
            _coneModel = coneModel;
            _fov = fov;
            _offsetX = offsetX;
            _offsetY = offsetY;
            _scale = scale;
            LoadLogFile(logFileName);
        }

        public void LoadLogFile(string path)
        {
            try
            {
                status = "Parsing...";
                Console.Out.Write("Parsing " + path + "...");
                TextReader tr = new StreamReader(path);
                //List<double[]> stateDoubles = new List<double[]>();
                // List<double[]> readingDoubles = new List<double[]>();
                // List<double[]> velocityDoubles = new List<double[]>();
                double x, y, t;
                
                string line = null;
                while ((line = tr.ReadLine()) != null)
                {
                    string[] parts = line.Split(' ');
                    if ((parts[0] == "Odometry" || parts[0] == "ODOM") && parts.Length >= 4)
                    {
                        x = double.Parse(parts[1], CultureInfo.InvariantCulture) * 100;
                        y = double.Parse(parts[2], CultureInfo.InvariantCulture) * 100;
                        t = double.Parse(parts[3], CultureInfo.InvariantCulture);
                        if (t > Math.PI)
                        {
                            t -= Math.PI*2;
                        }
                        if (t < -Math.PI)
                        {
                            t += Math.PI*2;
                        }
                        if (x < minX)
                            minX = x;
                        if (y < minY)
                            minY = y;
                        if (x > maxX)
                            maxX = x;
                        if (y > maxY)
                            maxY = y;
                        _stateDoubles.Add(new double[] {x, y, t});
                    }
                    else if (parts[0] == "Laser" || parts[0] == "FLASER")
                    {
                        numBeams = int.Parse(parts[1]);
                        if (parts.Length > numBeams + 2)
                        {
                            double[] readings = new double[numBeams];
                            for (int i = 0; i < numBeams; i++)
                            {
                                readings[i] = double.Parse(parts[i + 2], CultureInfo.InvariantCulture) * 100;
                                if (readings[i] > maxRange)
                                    maxRange = readings[i];
                            }
                            _readingDoubles.Add(readings);
                        }
                    }
                    Console.Out.Write(".");
                }
                tr.Close();
                Console.Out.WriteLine("");
                Console.Out.WriteLine("Done.");
                Console.Out.WriteLine("Raw Min(X,Y): " + minX + "," + minY + "  Max(X,Y): " + maxX + "," + maxY +
                                      " maxRange: " + maxRange);
                Console.Out.WriteLine("Map Size for states: " + (maxX - minX) + " " + (maxY - minY));
                base.GlobalMap = new Map((int)Math.Round((maxX - minX) / _scale), (int)Math.Round((maxY - minY) / _scale), 0.6, _scale);
                base.LocalMap = new Map(base.GlobalMap.Width, base.GlobalMap.Height, 0.6, _scale);

                Console.Out.WriteLine("Looking for duplicates");

                List<int> duplicateIndexes = new List<int>();

                for (int i = 0; i < _stateDoubles.Count; i++)
                {
                    if (i + 1 < _stateDoubles.Count && i + 1 < _readingDoubles.Count)
                    {
                        bool isDuplicate = true;
                        for (int s = 0; s < _stateDoubles[i].Length; s++)
                        {
                            if (_stateDoubles[i][s] != _stateDoubles[i + 1][s])
                            {
                                isDuplicate = false;
                                break;
                            }
                        }
                        /* if (isDuplicate)
                         {
                             for (int r = 0; r < _readingDoubles[i].Length; r++)
                             {
                                 if (_readingDoubles[i][r] != _readingDoubles[i + 1][r])
                                 {
                                     isDuplicate = false;
                                     break;
                                 }
                             }
                         }*/
                        if (isDuplicate)
                        {
                            duplicateIndexes.Add(i + 1);
                            // Console.Out.WriteLine(i + " same as " + (i + 1));
                        }
                    }
                }

                if (duplicateIndexes.Count > 0)
                {
                    int numRemoved = 0;
                    foreach (int i in duplicateIndexes)
                    {
                        _stateDoubles.RemoveAt(i - numRemoved);
                        _readingDoubles.RemoveAt(i - numRemoved++);
                    }
                }

                Console.Out.WriteLine("Removed " + duplicateIndexes.Count + " duplicates ");

                _stateDoubles.ForEach(delegate(double[] doubles)
                                          {
                                              doubles[0] -= minX;
                                              doubles[1] -= minY;
                                              // Console.Out.WriteLine("State: " + doubles[0] + "," + doubles[1] + "," + doubles[2]);
                                          });

                Console.Out.WriteLine("Adjusted States.");
                Console.Out.WriteLine("Calculating Velocities...");

                for (int i = 0; i < _stateDoubles.Count; i++)
                {
                    if (i + 1 < _stateDoubles.Count)
                    {
                        double dist = Math.Sqrt(Math.Pow(_stateDoubles[i][0] - _stateDoubles[i + 1][0], 2) +
                                                Math.Pow(_stateDoubles[i][1] - _stateDoubles[i + 1][1], 2));
                        //double angle =
                        //   Math.Atan2(_stateDoubles[i + 1][1] - _stateDoubles[i][1],
                        //              _stateDoubles[i + 1][0] - _stateDoubles[i][0]);
                        // Console.Out.WriteLine("Move from " + stateDoubles[i][0] + "," + stateDoubles[i][1] + " to " + stateDoubles[i+1][0] + "," + stateDoubles[i+1][1] + " is " + dist + " " + angle);
                        //if (angle == 0)
                        //{
                        double angle = _stateDoubles[i + 1][2] - _stateDoubles[i][2]; // hack
                        //}
                        if (angle < -Math.PI)
                        {
                            angle += Math.PI*2;
                        }
                        if (angle > Math.PI)
                        {
                            angle -= Math.PI*2;
                        }
                        _velocityDoubles.Add(new double[] {dist, angle});
                        // Console.Out.WriteLine("Velocity: " + _velocityDoubles[_velocityDoubles.Count - 1][0] + "," + _velocityDoubles[_velocityDoubles.Count - 1][1]);
                    }
                    else
                    {
                        Console.Out.WriteLine("Adding a termination velocity");
                        _velocityDoubles.Add(new double[] {0, 0});
                    }
                }
                Console.Out.Write(_stateDoubles.Count + " states, " + _readingDoubles.Count + " scans, " +
                                  _velocityDoubles.Count + " vectors");
                Console.Out.WriteLine("All done.");


                // make a sensor array
                if (numBeams > 0)
                {
                    // asume that these are lazers, with a spead in degrees of numBeams
                   // BeamModel bm = new BeamModel(200, 0.01, 800, 0.5, 0.2, 0.2, 0.1);
                   // bm.Prepare();
                   // ConeModel1Gaussian cm = new ConeModel1Gaussian(800, 0.05, 0.00005, 10, 0.4, 0.1);
                    double start = -(_fov/2);
                    double step = _fov/numBeams;// Math.PI / 180;
                    for (int i = 0; i < numBeams; i++)
                    {
                        base.DistanceSensors.Add(new DistanceSensor(0, 0, start + step*i, _beamModel, _coneModel));
                    }
                }

                base.Pose2DCurrent =
                    new Pose2D(_stateDoubles[_currentStep][0], _stateDoubles[_currentStep][1], _stateDoubles[_currentStep][2]);
                status = "Done";
                numOdometry = _stateDoubles.Count;
                numScans = _readingDoubles.Count;
                success = true;
            } catch(Exception)

            {
                status = "Parse Failure";
                Console.Out.WriteLine("Could not find file, or parse problem");
            }
            
        }

        public override DistanceReading[] GetDistanceReadings(Map map, Pose2D state, double percentNoise)
        {
            if (map == base.GlobalMap)
            {
                // cheat and get the result from the log file
                List<DistanceReading> readings = new List<DistanceReading>();
                for (int i = 0; i < base.DistanceSensors.Count; i++ )
                {
                    readings.Add(base.DistanceSensors[i].CreateReading(_readingDoubles[_currentStep][i]));
                }
                return readings.ToArray();
            } else
            {
                //
                return base.GetDistanceReadings(map, state, percentNoise);
            }
            
        }

        public override void Move(Velocity vel)
        {
            if (_currentStep + 1 >= _stateDoubles.Count)
            {
                // panic
                base.Velocity = new Velocity(0, 0);
                _currentStep = 0;
            }
            else
            {
                base.Pose2DCurrent =
                    new Pose2D(_stateDoubles[++_currentStep][0], _stateDoubles[_currentStep][1],
                                _stateDoubles[_currentStep][2]);
                base.Velocity =
                    new Velocity(_velocityDoubles[_currentStep - 1][0], _velocityDoubles[_currentStep - 1][1]);
                base.MotionModel.PrepareForSampling(base.Velocity, new TimeSpan(0, 0, 0, 1));
            }
        }
    }
}
